%ptCloudIn = pcread('../VIC/infrastructure-side/cooperative-vehicle-infrastructure-infrastructure-side-velodyne/000009.pcd');


ptCloudIn = pcread('../VIC/vehicle-side/cooperative-vehicle-infrastructure-vehicle-side-velodyne/000009.pcd');


%center
%dimensions
%orientation

%params = [11.4873085 10.59969 -1.613766 ...
%    1.6712 10.3220 5.75755...
%    90, 50, 0.017451];


params=[
    17.42089,-0.3785723,-1.071184... 
    1.575413,2.107992,4.588398... 
    0 0 0.116342]




model = cuboidModel(params);

indices = findPointsInsideCuboid(model,ptCloudIn);

cubPtCloud = select(ptCloudIn,indices);

% pcshowpair(ptCloudIn,cubPtCloud)
pcshowpair(ptCloudIn,ptCloudIn)

pcshowpair(ptCloudIn,cubPtCloud)


xlim([-30 30])
ylim([-30 40])
legend("Input Point Cloud","Enclosed Points",'TextColor',[1 1 1])